/*
 * @Author: your name
 * @Date: 2020-03-03 11:41:10
 * @LastEditTime: 2020-03-05 22:48:27
 * @LastEditors: Please set LastEditors
 * @Description: In User Settings Edit
 * @FilePath: /platform_controller_rviz/src/controller_pannel.cpp
 */

#include "platform_controller_rviz/controller_pannel.h"

#include <QLineEdit>
#include <QVBoxLayout>
#include <QHBoxLayout>
#include <QLabel>
#include <QGroupBox>
#include <QGridLayout>
#include <QCheckBox>
#include <QButtonGroup>
#include <QRadioButton>
#include <QPushButton>
#include <QMessageBox>

namespace cp{

ControllerPannel::ControllerPannel( QWidget* parent)
    : rviz::Panel(parent)
    , room_num_(301)
{
    initUI();
    initTopic();
    connect(button_group_,SIGNAL(buttonClicked(int)),this,SLOT(onRadionBtnGroupClicked(int)));
    connect(begin_btn_,SIGNAL(clicked()),this,SLOT(onBeginPatrolBtnClicked()));
    connect(stop_btn_,SIGNAL(clicked()),this,SLOT(onStopPatrolBtnClicked()));
    connect(load_patients_btn_,SIGNAL(clicked()),this,SLOT(onLoadPatientsBtnClicked()));

    connect(record_rosbag_btn_,SIGNAL(clicked()),this,SLOT(onRosbagRecordClicked()));
    connect(add_patrol_point_,SIGNAL(clicked()),this,SLOT(onAddPatrolPointClicked()));
    connect(stop_record_rosbag_btn_,SIGNAL(clicked()),this,SLOT(onStopRosbagRecordClicked()));

    connect(load_patorl_points_btn_,SIGNAL(clicked()),this,SLOT(onLoadPatrolPointsClicked()));
    connect(load_patients_btn_,SIGNAL(clicked()),this,SLOT(onLoadPatientsBtnClicked()));

    connect(begin_navigation_,SIGNAL(clicked()),this,SLOT(onBeginNavigationClicked()));
    connect(stop_navigation_,SIGNAL(clicked()),this,SLOT(onStopNavigationClicked()));
}

void ControllerPannel::load( const rviz::Config& config )
{
    
}
void ControllerPannel::save( rviz::Config config ) const
{

}


void ControllerPannel::initUI()
{
    QVBoxLayout* mainLayout = new QVBoxLayout();
    
    //数据包录制
    QGroupBox* rosbagBox = new QGroupBox(QStringLiteral("数据包录制"));
    QHBoxLayout* hlayout1  = new QHBoxLayout();
    rosbag_label_ = new QLabel(QStringLiteral("数据包名称"));
    rosbag_file_ = new QLineEdit();
    rosbag_file_->setText("monday");
    hlayout1->addWidget(rosbag_label_,1);
    hlayout1->addWidget(rosbag_file_,2);

    patrol_room_label_ = new  QLabel(QStringLiteral("房间号"));
    patrol_room_id_ = new QLineEdit();
    patrol_room_id_->setText(QString::number(room_num_));
    QHBoxLayout* hlayout3  = new QHBoxLayout();
    hlayout3->addWidget(patrol_room_label_,1);
    hlayout3->addWidget(patrol_room_id_,2);


    QHBoxLayout* hlayout2  = new QHBoxLayout();
    record_rosbag_btn_ = new QPushButton(QStringLiteral("开始录制"));
    add_patrol_point_ = new QPushButton(QStringLiteral("添加送药点"));
    stop_record_rosbag_btn_= new QPushButton(QStringLiteral("停止录制"));
    hlayout2->addWidget(record_rosbag_btn_);
    hlayout2->addWidget(add_patrol_point_);
    hlayout2->addWidget(stop_record_rosbag_btn_);

    QVBoxLayout* rosbag_layout  = new QVBoxLayout();
    rosbag_layout->addLayout(hlayout1);
    rosbag_layout->addLayout(hlayout3);
    rosbag_layout->addLayout(hlayout2);

    rosbagBox->setLayout(rosbag_layout);

    //加载数据
    QGroupBox* dataLoadBox = new QGroupBox(QStringLiteral("导入送餐文件"));

    patorl_points_file_ = new QLineEdit();
    patorl_points_file_->setText("monday.json");
    load_patorl_points_btn_   = new QPushButton(QStringLiteral("加载送药点文件"));
    QHBoxLayout* patrolLayout = new QHBoxLayout();
    patrolLayout->addWidget(patorl_points_file_,2);
    patrolLayout->addWidget(load_patorl_points_btn_,1);

    patients_file_ = new QLineEdit();
    patients_file_->setText("patient.csv");
    load_patients_btn_ = new QPushButton(QStringLiteral("加载病人信息"));
    QHBoxLayout* patientBtnLayout = new QHBoxLayout();
    patientBtnLayout->addWidget(patients_file_,2);
    patientBtnLayout->addWidget(load_patients_btn_,1);

    QVBoxLayout* dataLoad_layout  = new QVBoxLayout();
    dataLoad_layout->addLayout(patrolLayout);
    dataLoad_layout->addLayout(patientBtnLayout);
    dataLoadBox->setLayout(dataLoad_layout);


    //开启导航
    QGroupBox* navigationBox = new QGroupBox(QStringLiteral("开启导航"));
    map_label_ = new QLabel(QStringLiteral("地图文件"));
    map_file_ = new QLineEdit();
    map_file_->setText("patrol_map_yaml.yaml");
    begin_navigation_ = new QPushButton(QStringLiteral("打开导航"));
    stop_navigation_ = new QPushButton(QStringLiteral("关闭导航"));
    QHBoxLayout* lay0 = new QHBoxLayout();
    lay0->addWidget(map_label_,1);
    lay0->addWidget(map_file_,2);
    
    QHBoxLayout* lay1 = new QHBoxLayout();
    lay1->addWidget(begin_navigation_);
    lay1->addWidget(stop_navigation_);

    QVBoxLayout* v_layout  = new QVBoxLayout();
    v_layout->addLayout(lay0);
    v_layout->addLayout(lay1);
    navigationBox->setLayout(v_layout);


    //送药
    // QGroupBox* radionBtnBox = new QGroupBox(QStringLiteral("区域选择"));
    button_group_ = new QButtonGroup(this);
    full_region_   = new QRadioButton(QStringLiteral("全选")); 
    top_region_   = new QRadioButton(QStringLiteral("上半区"));
    btmRegion_    = new QRadioButton(QStringLiteral("下半区"));
    any_region_   = new QRadioButton(QStringLiteral("自由选择"));
    any_region_->setChecked(true);
    button_group_->addButton(full_region_,0);
    button_group_->addButton(top_region_,1);
    button_group_->addButton(btmRegion_,2);
    button_group_->addButton(any_region_,3);
    button_group_->setExclusive(true);

    QHBoxLayout* radioBtnLayout = new QHBoxLayout();
    radioBtnLayout->addWidget(full_region_);
    radioBtnLayout->addWidget(top_region_);
    radioBtnLayout->addWidget(btmRegion_);
    radioBtnLayout->addWidget(any_region_);

    
    QGroupBox* topBox = new QGroupBox(QStringLiteral("上半区"));
    QGroupBox* btmBox = new QGroupBox(QStringLiteral("下半区"));

    QGridLayout* topLayout = new QGridLayout();
    QGridLayout* btmLayout = new QGridLayout();

    int col = 6;
    for(int i=0;i<24;i++)
    {
        QCheckBox*chkBox = new QCheckBox("hello");
        topLayout->addWidget(chkBox,i/col,i%col);
        checkBoxLists_.push_back(chkBox);
    }
    topBox->setLayout(topLayout);

    for(int i=0;i<24;i++)
    {
        QCheckBox*chkBox = new QCheckBox("hello");
        btmLayout->addWidget(chkBox,i/col,i%col);
        checkBoxLists_.push_back(chkBox);
    }
    btmBox->setLayout(btmLayout);

    mainLayout->addWidget(rosbagBox);

    mainLayout->addWidget(dataLoadBox);

    mainLayout->addWidget(navigationBox);

    mainLayout->addLayout(radioBtnLayout);
    
    mainLayout->addWidget(topBox);

    mainLayout->addWidget(btmBox);

    QHBoxLayout* btnLayout = new QHBoxLayout();
    begin_btn_ = new QPushButton(QStringLiteral("开始送药"));
    stop_btn_ = new QPushButton(QStringLiteral("停止送药"));
    btnLayout->addWidget(begin_btn_);
    btnLayout->addWidget(stop_btn_);
    mainLayout->addLayout(btnLayout);

    setLayout(mainLayout);
}


void ControllerPannel::initTopic()
{
    //数据包录制
    rosbag_record_pub_      = nh_.advertise<std_msgs::String>("/launch_rosbag_record",1);
    stop_rosbag_record_pub_ = nh_.advertise<std_msgs::Empty>("/close_rosbag_record",1);
    add_patrol_point_pub_   = nh_.advertise<std_msgs::String>("/add_patrol_point",1);
    save_patrol_points_pub_ = nh_.advertise<std_msgs::String>("/save_patrol_point",1);

    // 文件加载
    load_patients_pub_      = nh_.advertise<std_msgs::String>("load_patients",1);
    load_patrol_points_pub_ = nh_.advertise<std_msgs::String>("/loadPoints",1);
    patients_sub_           = nh_.subscribe<platform_controller_msgs::Patients>("patients",1,&ControllerPannel::onPatientsCallback,this);
    
    //导航
    begin_navigation_pub_ = nh_.advertise<std_msgs::String>("/launch_amcl",1);
    end_navigation_pub_   = nh_.advertise<std_msgs::Empty>("/close_amcl",1);

    //送餐
    stop_patrol_pub_    = nh_.advertise<std_msgs::Bool>("isEnablePatrol",1);
    patrol_pub_         = nh_.advertise<platform_controller_msgs::PatrolTask>("patrol_task",1);

    //操作结果
    operation_result_sub_ = nh_.subscribe<std_msgs::String>("operation_result",1,&ControllerPannel::onOperationResultCallback,this);
}

void ControllerPannel::onPatientsCallback(const platform_controller_msgs::Patients::ConstPtr& msg)
{
    patients_ = (*msg);

    char label[256] = {0};
    //更新checkbox
    for(int i=0; i < msg->patients.size(); i++)
    {
        memset(label,0,256);
        sprintf(label,"%s_%d",msg->patients.at(i).name.c_str(),msg->patients.at(i).room_num);
        std::cout<<msg->patients.at(i).name<<" "<<msg->patients.at(i).room_num<<" "<<label<<std::endl;
        checkBoxLists_.at(msg->patients.at(i).id)->setText(QString::fromLocal8Bit(std::string(label).data()));
    }
}


void ControllerPannel::onOperationResultCallback(const std_msgs::String::ConstPtr& msg)
{
    QMessageBox::information(this,QStringLiteral("Result"),QString::fromLocal8Bit(msg->data.c_str()));
}


void ControllerPannel::onRadionBtnGroupClicked(int id)
{
    ROS_INFO("radion button: %d clicked.",id);
    if(id == 0)
    {
        //全选
        for(int i=0;i<checkBoxLists_.size();i++)
        {
            checkBoxLists_.at(i)->setCheckState(Qt::Checked);
        }
    }
    else if(id == 1)
    {
        //上半区
        for(int i=0;i<checkBoxLists_.size()/2;i++)
        {
            checkBoxLists_.at(i)->setCheckState(Qt::Checked);
        }

        for(int i=checkBoxLists_.size()/2;i<checkBoxLists_.size();i++)
        {
            checkBoxLists_.at(i)->setCheckState(Qt::Unchecked);
        }

    }
    else if(id ==2 )
    {
        //下半区
        for(int i=0;i<checkBoxLists_.size()/2;i++)
        {
            checkBoxLists_.at(i)->setCheckState(Qt::Unchecked);
        }

        for(int i=checkBoxLists_.size()/2;i<checkBoxLists_.size();i++)
        {
            checkBoxLists_.at(i)->setCheckState(Qt::Checked);
        }
    }
    else if(id == 3)
    {
        for(int i=0;i<checkBoxLists_.size();i++)
        {
            checkBoxLists_.at(i)->setCheckState(Qt::Unchecked);
        }
    }
}

void ControllerPannel::onBeginPatrolBtnClicked()
{
    platform_controller_msgs::PatrolTask patrol_task;
    patrol_task.mode = 2;
    patrol_task.time = 15;
    //查看当前选中的checkBox
    for(int i=0;i<checkBoxLists_.size();i++)
    {
        if(checkBoxLists_.at(i)->checkState() == Qt::Checked)
        {
            patrol_task.index.push_back(i);
        }
    }
    patrol_pub_.publish(patrol_task);
}

void ControllerPannel::onStopPatrolBtnClicked()
{
    std_msgs::Bool stop_patrol;
    stop_patrol.data = false;
    stop_patrol_pub_.publish(stop_patrol);
}

void ControllerPannel::onRosbagRecordClicked()
{
    ROS_INFO("Record rosbag to file");
    std_msgs::String rosbag_name;
    rosbag_name.data = rosbag_file_->text().toStdString();
    rosbag_record_pub_.publish(rosbag_name);
}


void ControllerPannel::onStopRosbagRecordClicked()
{
    ROS_INFO("Stop record rosbag to file");
    std_msgs::Empty stop_record;
    stop_rosbag_record_pub_.publish(stop_record);

    //保存巡逻点
    ros::Duration(2.0).sleep();
    std_msgs::String save_patrol_points;
    save_patrol_points.data = rosbag_file_->text().toStdString() + std::string(".json");
    save_patrol_points_pub_.publish(save_patrol_points);
}

void ControllerPannel::onAddPatrolPointClicked()
{
    ROS_INFO("Add patrol points to file");
    std_msgs::String patrol_points_room_id;
    patrol_points_room_id.data = patrol_room_id_->text().toStdString();
    add_patrol_point_pub_.publish(patrol_points_room_id);
    
    //房间号+1
    room_num_ += 1;
    patrol_room_id_->setText(QString::number(room_num_));
}

void ControllerPannel::onLoadPatrolPointsClicked()
{
    if(patorl_points_file_->text().isEmpty())
        return;

    ROS_INFO("Load patrol points");
    std_msgs::String patrol_points_name;
    patrol_points_name.data = patorl_points_file_->text().toStdString();
    load_patrol_points_pub_.publish(patrol_points_name);
}


void ControllerPannel::onLoadPatientsBtnClicked()
{
    if(patorl_points_file_->text().isEmpty())
        return;
    ROS_INFO("Load patient information from file");
    QString patients_file  = patients_file_->text();
    std_msgs::String loadPatientsMsg;
    loadPatientsMsg.data = patients_file.toStdString();
    load_patients_pub_.publish(loadPatientsMsg);
}

void ControllerPannel::onBeginNavigationClicked()
{
    std_msgs::String navigation_msg;
    navigation_msg.data = map_file_->text().toStdString();
    begin_navigation_pub_.publish(navigation_msg);
}

void ControllerPannel::onStopNavigationClicked()
{
    std_msgs::Empty close_navigation_msg;
    end_navigation_pub_.publish(close_navigation_msg);
}



}

#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(cp::ControllerPannel,rviz::Panel )